﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace WindowsFormsApp1
{
    public partial class Form1 : Form
    {
        public Form1()
        {
            InitializeComponent();
        }

        private void Form1_Load(object sender, EventArgs e)
        {

        }

        private void button1_Click(object sender, EventArgs e)
        {
            //类型转化
            double[] x = { Convert.ToDouble(t1.Text), Convert.ToDouble(t6.Text), Convert.ToDouble(t11.Text), Convert.ToDouble(t16.Text) };

            double[] y = { Convert.ToDouble(t2.Text), Convert.ToDouble(t7.Text), Convert.ToDouble(t12.Text), Convert.ToDouble(t17.Text) };

            double[] X = { Convert.ToDouble(t3.Text), Convert.ToDouble(t8.Text), Convert.ToDouble(t13.Text), Convert.ToDouble(t18.Text) };

            double[] Y = { Convert.ToDouble(t4.Text), Convert.ToDouble(t9.Text), Convert.ToDouble(t14.Text), Convert.ToDouble(t19.Text) };

            double[] Z = { Convert.ToDouble(t5.Text), Convert.ToDouble(t10.Text), Convert.ToDouble(t15.Text), Convert.ToDouble(t20.Text) };

            double f = 0.15324, _m = 40000.0;

            for (int i = 0; i < 4; i++)    //单位换算

            {

                x[i] = x[i] / 1000;

                y[i] = y[i] / 1000;

            }




            // 定义外方位元素，并附初值


            double Xs, Ys, Zs, phi = 0, omiga = 0, kappa = 0;

            Xs = (X[0] + X[1] + X[2] + X[3]) / 4.0;

            Ys = (Y[0] + Y[1] + Y[2] + Y[3]) / 4.0;

            Zs = _m * f;




            // 定义x,y近似值,即计算值


            double[] _x = new double[4];

            double[] _y = new double[4];




            // 定义共线方程中的分子分母项，便于计算


            double[] _X = new double[4];

            double[] _Y = new double[4];

            double[] _Z = new double[4];




            // 定义旋转矩阵R


            double[,] R = new double[3, 3];

            double[,] L = new double[8, 1];//误差方程中的常数项

            double[,] XX = new double[6, 1];//X向量




            // 开始迭代


            do

            {


                // 计算旋转矩阵


                R[0, 0] = Math.Cos(phi) * Math.Cos(kappa) - Math.Sin(phi) * Math.Sin(omiga) * Math.Sin(kappa);//a1

                R[0, 1] = -Math.Cos(phi) * Math.Sin(kappa) - Math.Sin(phi) * Math.Sin(omiga) * Math.Cos(kappa);//a2

                R[0, 2] = -Math.Sin(phi) * Math.Cos(omiga);//a3

                R[1, 0] = Math.Cos(omiga) * Math.Sin(kappa);//b1

                R[1, 1] = Math.Cos(omiga) * Math.Cos(kappa);//b2

                R[1, 2] = -Math.Sin(omiga);//b3

                R[2, 0] = Math.Sin(phi) * Math.Cos(kappa) + Math.Cos(phi) * Math.Sin(omiga) * Math.Sin(kappa);//c1

                R[2, 1] = -Math.Sin(phi) * Math.Sin(kappa) + Math.Cos(phi) * Math.Sin(omiga) * Math.Cos(kappa);//c2

                R[2, 2] = Math.Cos(phi) * Math.Cos(omiga);//c3



                for (int i = 0; i < 4; i++)

                {

                    //用共线方程计算 x，y 的近似值 ,即计算值      

                    _X[i] = R[0, 0] * (X[i] - Xs) + R[1, 0] * (Y[i] - Ys) + R[2, 0] * (Z[i] - Zs);

                    _Y[i] = R[0, 1] * (X[i] - Xs) + R[1, 1] * (Y[i] - Ys) + R[2, 1] * (Z[i] - Zs);

                    _Z[i] = R[0, 2] * (X[i] - Xs) + R[1, 2] * (Y[i] - Ys) + R[2, 2] * (Z[i] - Zs);



                    _x[i] = -f * _X[i] / _Z[i];

                    _y[i] = -f * _Y[i] / _Z[i];

                }



                Matrix B = new Matrix(8, 6, "B");//4个控制点，每个是2行6列，4个是8行6列

                int n = B.getN;

                int m = B.getM;

                double[,] b = B.Detail;

                for (int i = 0; i < 4; i++)
                
                {

                    //计算系数矩阵

                    b[2 * i, 0] = (R[0, 0] * f + R[0, 2] * x[i]) / _Z[i];

                    b[2 * i, 1] = (R[1, 0] * f + R[1, 2] * x[i]) / _Z[i];

                    b[2 * i, 2] = (R[2, 0] * f + R[2, 2] * x[i]) / _Z[i];

                    b[2 * i, 3] = y[i] * Math.Sin(omiga) - ((x[i] / f) * (x[i] * Math.Cos(kappa) - y[i] * Math.Sin(kappa)) + f * Math.Cos(kappa)) * Math.Cos(omiga);

                    b[2 * i, 4] = -f * Math.Sin(kappa) - (x[i] / f) * (x[i] * Math.Sin(kappa) + y[i] * Math.Cos(kappa));

                    b[2 * i, 5] = y[i];



                    b[2 * i + 1, 0] = (R[0, 1] * f + R[0, 2] * y[i]) / _Z[i];

                    b[2 * i + 1, 1] = (R[1, 1] * f + R[1, 2] * y[i]) / _Z[i];

                    b[2 * i + 1, 2] = (R[2, 1] * f + R[2, 2] * y[i]) / _Z[i];

                    b[2 * i + 1, 3] = -x[i] * Math.Sin(omiga) - ((x[i] / f) * (x[i] * Math.Cos(kappa) - y[i] * Math.Sin(kappa)) - f * Math.Sin(kappa)) * Math.Cos(omiga);

                    b[2 * i + 1, 4] = -f * Math.Cos(kappa) - (y[i] / f) * (x[i] * Math.Sin(kappa) + y[i] * Math.Cos(kappa));

                    b[2 * i + 1, 5] = -x[i];



                    //计算常数项

                    L[2 * i, 0] = x[i] - _x[i];

                    L[2 * i + 1, 0] = y[i] - _y[i];

                }

               

            //矩阵类


            Matrix C = MatrixOperator.MatrixTrans(B);         //系数矩阵的转置矩阵

                C.Name = "C";

                Matrix D = MatrixOperator.MatrixMulti(C, B);       //系数矩阵与其转置矩阵相乘

                D.Name = "C*B";

                Matrix dn = MatrixOperator.MatrixInvByCom(D);      //系数矩阵与其转置矩阵积的逆矩阵

                dn.Name = "dn";

                Matrix dn2 = MatrixOperator.MatrixMulti(dn, C);       //ATA的逆阵乘以A的转置

                dn2.Name = "dn2";

                double[,] ATARAT = dn2.Detail;



                //计算ATARAT* L，存在XX中


                for (int i = 0; i < 6; i++)

                    for (int j = 0; j < 1; j++)

                    {

                        XX[i, j] = 0;

                        for (int l = 0; l < 8; l++)

                            XX[i, j] += ATARAT[i, l] * L[l, 0];

                    }





                // 计算外方位元素值

                Xs += XX[0, 0];

                Ys += XX[1, 0];

                Zs += XX[2, 0];

                phi += XX[3, 0];

                omiga += XX[4, 0];

                kappa += XX[5, 0];


                //计算

            } while (Math.Abs(XX[0, 0]) >= 0.000029 || Math.Abs(XX[1, 0]) >= 0.000029 || Math.Abs(XX[2, 0]) >= 0.000029 || Math.Abs(XX[3, 0]) >= 1000 * 0.000029 || Math.Abs(XX[4, 0]) >= 1000 * 0.000029 || Math.Abs(XX[5, 0]) >= 1000 * 0.000029);

            //窗口
            Form2 form2 = new Form2(Xs, Ys, Zs, phi, omiga, kappa,R);
            form2.Show();
        }
    }
}
